/**
 * Copyright (C) BFH www.bfh.ch 2011
 * Code written by: Patrick Dobler, Marc Folly
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
package ch.bfh.ti.kybernetik.engine.controller.roboter;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.LinkedList;
import java.util.List;

import javax.vecmath.Point2d;
import javax.vecmath.Vector2d;

import org.apache.commons.collections.BufferUtils;
import org.apache.commons.collections.buffer.CircularFifoBuffer;

import ch.bfh.ti.kybernetik.engine.controller.Simulator;
import ch.bfh.ti.kybernetik.engine.model.Motor;
import ch.bfh.ti.kybernetik.engine.model.Roboter;
import ch.bfh.ti.kybernetik.engine.model.RoboterMove;
import ch.bfh.ti.kybernetik.lego.ki.RoboterKI;

/**
 * The {@link Simulator} default {@link RoboterController} implementation
 * 
 */
class DefaultRoboterControllerImpl implements RoboterController {

	private static final int ROBOTER_MOVE_HISTORY_SIZE = 500;

	private static volatile double DELTA_T = 0.004;

	private final RoboterConstruction roboterConstruction;

	private final Roboter roboter;

	private RoboterKI roboterKI;

	@SuppressWarnings("unchecked")
	private Collection<RoboterMove> roboterMoveStates = BufferUtils.synchronizedBuffer(new CircularFifoBuffer(ROBOTER_MOVE_HISTORY_SIZE));

	public DefaultRoboterControllerImpl(Roboter roboter, RoboterKI roboterKI, RoboterConstruction roboterConstruction) {
		this.roboter = roboter;
		this.roboterConstruction = roboterConstruction;
		this.roboterKI = roboterKI;
	}

	private RoboterMove calculateNextRoboterMove(RoboterConstruction roboterConstruction) {
		Point2d leftSpeedPoint = getRoboterMotorSpeedPoint(roboter.getLeftMotor());
		Point2d rightSpeedPoint = getRoboterMotorSpeedPoint(roboter.getRightMotor());

		double newRoboterPointX = (leftSpeedPoint.getX() + rightSpeedPoint.getX()) / 2;
		double newRoboterPointY = (leftSpeedPoint.getY() + rightSpeedPoint.getY()) / 2;
		Point2d newRoboterPoint = new Point2d(newRoboterPointX, newRoboterPointY);

		Vector2d topVektor = new Vector2d(rightSpeedPoint.getX() - leftSpeedPoint.getX(), rightSpeedPoint.getY() - leftSpeedPoint.getY());
		topVektor.normalize();
		Vector2d newDirectionVector = new Vector2d(topVektor.getY(), -topVektor.getX());

		RoboterMove rmove = new RoboterMove(roboter, newRoboterPoint, newDirectionVector);

		return rmove;
	}

	private Point2d getRoboterMotorSpeedPoint(Motor motor) {
		Point2d motorPoint = roboterConstruction.getRoboterElementPoint(motor);
		double x = motorPoint.getX() + DELTA_T * motor.getVelocity() * roboter.getDirection().getX();
		double y = motorPoint.getY() + DELTA_T * motor.getVelocity() * roboter.getDirection().getY();
		return new Point2d(x, y);
	}

	private RoboterMove calculateNextRoboterMove() {
		return calculateNextRoboterMove(roboterConstruction);
	}

	@Override
	public RoboterMove calculateNewRoboterPosition(double leftLightSensor, double rightLightSensor) {
		// START TODO
		roboterKI.setLightMin(0);
		roboterKI.setLightMax((int) roboter.getLeftLightSensor().getMaxLightIntensity());
		roboterKI.calculateNewRoboterMotorSpeed((int) leftLightSensor, (int) rightLightSensor);
		double newLeftMotorSpeed = roboterKI.getMotorSpeedLeft();
		double newRightMotorSpeed = roboterKI.getMotorSpeedRight();

		roboter.getLeftMotor().setVelocity(newLeftMotorSpeed);
		roboter.getRightMotor().setVelocity(newRightMotorSpeed);
		// END TODO

		final RoboterMove currentRoboterMoveState = calculateNextRoboterMove();

		this.roboterMoveStates.add(currentRoboterMoveState);
		roboter.setX(currentRoboterMoveState.getNewRoboterPoint().getX());
		roboter.setY(currentRoboterMoveState.getNewRoboterPoint().getY());
		roboter.setDirection(currentRoboterMoveState.getNewDirectionVector());
		return currentRoboterMoveState;
	}

	@Override
	public List<RoboterMove> calculateNextEstimatedRoboterMoves(int count) {
		Roboter iterationRoboter = new Roboter(this.roboter);
		List<RoboterMove> estimatedRoboterMoveStates = new LinkedList<RoboterMove>();
		for (int i = 0; i < count; i++) {
			DefaultRoboterConstructionImpl constructionImpl = new DefaultRoboterConstructionImpl(iterationRoboter);
			RoboterMove roboterMoveState = calculateNextRoboterMove(constructionImpl);
			estimatedRoboterMoveStates.add(roboterMoveState);
			// Set new roboter data
			iterationRoboter.setX(roboterMoveState.getNewRoboterPoint().getX());
			iterationRoboter.setY(roboterMoveState.getNewRoboterPoint().getY());
			iterationRoboter.setDirection(roboterMoveState.getNewDirectionVector());
		}
		return estimatedRoboterMoveStates;
	}

	@Override
	public Roboter getRoboter() {
		return roboter;
	}

	@Override
	public List<RoboterMove> getRoboterMoveHistory() {
		return Collections.unmodifiableList(new ArrayList<RoboterMove>(roboterMoveStates));
	}

	@Override
	public RoboterConstruction getRoboterConstruction() {
		return roboterConstruction;
	}

	@Override
	public void setDeltaT(double deltaT) {
		DELTA_T = deltaT;
	}

	@Override
	public double getDeltaT() {
		return DELTA_T;
	}

	@Override
	public RoboterKI getRoboterKI() {
		return roboterKI;
	}

	@Override
	public void setRoboterKI(RoboterKI roboterKI) {
		this.roboterKI = roboterKI;
	}

}
